#include "qtRbbSerial.h"
#include <string.h>

typedef enum {
	NO_FLUSH,
	FLUSH_SEND_BUF
} flush_bool_t;

typedef enum {
    NO_BLOCK,
    BLOCK
} block_bool_t;

#define RBB_SBUF_LEN 512

using namespace mopenocd;

qtRbbSerial::qtRbbSerial()
{
    _ser = nullptr;
    bitbang::buf_size = 256;
}

bool qtRbbSerial::open(const char *dev, long params)
{
    bool ret;

    if (params == 0)
        params = 115200;

    _ser = new QSerialPort;

    _ser->setPortName(dev);
    _ser->setBaudRate(params);

    _ser->setParity(QSerialPort::NoParity);
    _ser->setStopBits(QSerialPort::OneStop);
    _ser->setDataBits(QSerialPort::Data8);
    _ser->setFlowControl(QSerialPort::NoFlowControl);

    ret = _ser->open(QIODevice::ReadWrite);
    if (!ret)
    {
        delete _ser;
        _ser = nullptr;
    }

    return ret;
}

void qtRbbSerial::close()
{
    if (_ser == nullptr)
        return;
    _ser->close();
    delete _ser;
    _ser = nullptr;
}

void qtRbbSerial::bb_reset(int trst, int srst)
{
    char c = 'r' + ((trst ? 0x2 : 0x0) | (srst ? 0x1 : 0x0));
    /* Always flush the send buffer on reset, because the reset call need not be
     * followed by jtag_execute_queue(). */
    remote_bitbang_queue(c, FLUSH_SEND_BUF);
}

int qtRbbSerial::bb_write(int tck, int tms, int tdi)
{
	char c = '0' + ((tck ? 0x4 : 0x0) | (tms ? 0x2 : 0x0) | (tdi ? 0x1 : 0x0));

    if (_ser == nullptr)
        return -1;

	return remote_bitbang_queue(c, NO_FLUSH);
}

int qtRbbSerial::bb_sample()
{
    return remote_bitbang_queue('R', NO_FLUSH);;
}

int qtRbbSerial::bb_read_sample()
{
    int ret;

    if (_rbuf.size() == 0)
    {
        if (remote_bitbang_fill_buf(BLOCK) != ERROR_OK)
        {
            qDebug("rbb fill buf fail");
            return ERROR_FAIL;
        }
    }

    if (_rbuf.size() == 0)
        return ERROR_FAIL;

    ret = char_to_int(_rbuf.getChar());

    return ret;
}

void qtRbbSerial::bb_flush()
{
    remote_bitbang_flush();
}

int qtRbbSerial::char_to_int(int c)
{
    switch (c) {
        case '0':
            return BB_LOW;
        case '1':
            return BB_HIGH;
        default:
            //remote_bitbang_quit();
            qDebug("remote_bitbang: invalid read response: %c(%i)", c, c);
            return BB_ERROR;
    }
}

int qtRbbSerial::remote_bitbang_queue(char c, int flush)
{
    QByteArray b(1, c);

    _sbuf.append(b);

    if (flush == FLUSH_SEND_BUF || _sbuf.size() >= 64)
		return remote_bitbang_flush();

	return ERROR_OK;
}

int qtRbbSerial::remote_bitbang_flush(void)
{
    int ret = ERROR_FAIL;
    QByteArray d;

    if (_sbuf.size() == 0)
        return 0;
    if (_ser == nullptr)
        goto _out;

    d.resize(_sbuf.size());
    _sbuf.read(d.data(), d.length());

    //qDebug("wsize %d", d.length());
    ret = _ser->write(d);
    _ser->waitForBytesWritten();
    if (ret != d.length())
        ret = ERROR_FAIL;
    else
        ret = 0;

    _sbuf.clear();

_out:
    if (ret)
    {
        qDebug("write serial fail\n");
    }

    return ret;
}

/* Read any incoming data, placing it into the buffer. */
int qtRbbSerial::remote_bitbang_fill_buf(int block)
{
    if (block == BLOCK) {
        if (remote_bitbang_flush() != ERROR_OK)
            return ERROR_FAIL;

        if (!_ser->waitForReadyRead(2000))
            return ERROR_FAIL;
    }

    QByteArray rcv;

    rcv = _ser->readAll();
    if (rcv.length() > 0)
        _rbuf.append(rcv);

    return ERROR_OK;
}



#define MaxByteArraySize 2048

void QRingChunk::allocate(int alloc)
{
    Q_ASSERT(alloc > 0 && size() == 0);

    if (chunk.size() < alloc || isShared())
        chunk = QByteArray(alloc, Qt::Uninitialized);
}

void QRingChunk::detach()
{
    Q_ASSERT(isShared());

    const int chunkSize = size();
    QByteArray x(chunkSize, Qt::Uninitialized);
    ::memcpy(x.data(), chunk.constData() + headOffset, chunkSize);
    chunk = std::move(x);
    headOffset = 0;
    tailOffset = chunkSize;
}

QByteArray QRingChunk::toByteArray()
{
    if (headOffset != 0 || tailOffset != chunk.size()) {
        if (isShared())
            return chunk.mid(headOffset, size());

        if (headOffset != 0) {
            char *ptr = chunk.data();
            ::memmove(ptr, ptr + headOffset, size());
            tailOffset -= headOffset;
            headOffset = 0;
        }

        chunk.reserve(0); // avoid that resizing needlessly reallocates
        chunk.resize(tailOffset);
    }

    return chunk;
}

/*!
    \internal

    Access the bytes at a specified position the out-variable length will
    contain the amount of bytes readable from there, e.g. the amount still
    the same QByteArray
*/
const char *QRingBuffer::readPointerAtPosition(qint64 pos, qint64 &length) const
{
    Q_ASSERT(pos >= 0);

    for (const QRingChunk &chunk : buffers) {
        length = chunk.size();
        if (length > pos) {
            length -= pos;
            return chunk.data() + pos;
        }
        pos -= length;
    }

    length = 0;
    return nullptr;
}

void QRingBuffer::free(qint64 bytes)
{
    Q_ASSERT(bytes <= bufferSize);

    while (bytes > 0) {
        const qint64 chunkSize = buffers.constFirst().size();

        if (buffers.size() == 1 || chunkSize > bytes) {
            QRingChunk &chunk = buffers.first();
            // keep a single block around if it does not exceed
            // the basic block size, to avoid repeated allocations
            // between uses of the buffer
            if (bufferSize == bytes) {
                if (chunk.capacity() <= basicBlockSize && !chunk.isShared()) {
                    chunk.reset();
                    bufferSize = 0;
                } else {
                    clear(); // try to minify/squeeze us
                }
            } else {
                Q_ASSERT(bytes < MaxByteArraySize);
                chunk.advance(bytes);
                bufferSize -= bytes;
            }
            return;
        }

        bufferSize -= chunkSize;
        bytes -= chunkSize;
        buffers.removeFirst();
    }
}

char *QRingBuffer::reserve(qint64 bytes)
{
    Q_ASSERT(bytes > 0 && bytes < MaxByteArraySize);

    const int chunkSize = qMax(basicBlockSize, int(bytes));
    int tail = 0;
    if (bufferSize == 0) {
        if (buffers.isEmpty())
            buffers.append(QRingChunk(chunkSize));
        else
            buffers.first().allocate(chunkSize);
    } else {
        const QRingChunk &chunk = buffers.constLast();
        // if need a new buffer
        if (basicBlockSize == 0 || chunk.isShared() || bytes > chunk.available())
            buffers.append(QRingChunk(chunkSize));
        else
            tail = chunk.size();
    }

    buffers.last().grow(bytes);
    bufferSize += bytes;
    return buffers.last().data() + tail;
}

/*!
    \internal

    Allocate data at buffer head
*/
char *QRingBuffer::reserveFront(qint64 bytes)
{
    Q_ASSERT(bytes > 0 && bytes < MaxByteArraySize);

    const int chunkSize = qMax(basicBlockSize, int(bytes));
    if (bufferSize == 0) {
        if (buffers.isEmpty())
            buffers.prepend(QRingChunk(chunkSize));
        else
            buffers.first().allocate(chunkSize);
        buffers.first().grow(chunkSize);
        buffers.first().advance(chunkSize - bytes);
    } else {
        const QRingChunk &chunk = buffers.constFirst();
        // if need a new buffer
        if (basicBlockSize == 0 || chunk.isShared() || bytes > chunk.head()) {
            buffers.prepend(QRingChunk(chunkSize));
            buffers.first().grow(chunkSize);
            buffers.first().advance(chunkSize - bytes);
        } else {
            buffers.first().advance(-bytes);
        }
    }

    bufferSize += bytes;
    return buffers.first().data();
}

void QRingBuffer::chop(qint64 bytes)
{
    Q_ASSERT(bytes <= bufferSize);

    while (bytes > 0) {
        const qint64 chunkSize = buffers.constLast().size();

        if (buffers.size() == 1 || chunkSize > bytes) {
            QRingChunk &chunk = buffers.last();
            // keep a single block around if it does not exceed
            // the basic block size, to avoid repeated allocations
            // between uses of the buffer
            if (bufferSize == bytes) {
                if (chunk.capacity() <= basicBlockSize && !chunk.isShared()) {
                    chunk.reset();
                    bufferSize = 0;
                } else {
                    clear(); // try to minify/squeeze us
                }
            } else {
                Q_ASSERT(bytes < MaxByteArraySize);
                chunk.grow(-bytes);
                bufferSize -= bytes;
            }
            return;
        }

        bufferSize -= chunkSize;
        bytes -= chunkSize;
        buffers.removeLast();
    }
}

void QRingBuffer::clear()
{
    if (buffers.isEmpty())
        return;

    buffers.erase(buffers.begin() + 1, buffers.end());
    buffers.first().clear();
    bufferSize = 0;
}

qint64 QRingBuffer::indexOf(char c, qint64 maxLength, qint64 pos) const
{
    Q_ASSERT(maxLength >= 0 && pos >= 0);

    if (maxLength == 0)
        return -1;

    qint64 index = -pos;
    for (const QRingChunk &chunk : buffers) {
        const qint64 nextBlockIndex = qMin(index + chunk.size(), maxLength);

        if (nextBlockIndex > 0) {
            const char *ptr = chunk.data();
            if (index < 0) {
                ptr -= index;
                index = 0;
            }

            const char *findPtr = reinterpret_cast<const char *>(memchr(ptr, c,
                                                                        nextBlockIndex - index));
            if (findPtr)
                return qint64(findPtr - ptr) + index + pos;

            if (nextBlockIndex == maxLength)
                return -1;
        }
        index = nextBlockIndex;
    }
    return -1;
}

qint64 QRingBuffer::read(char *data, qint64 maxLength)
{
    const qint64 bytesToRead = qMin(size(), maxLength);
    qint64 readSoFar = 0;
    while (readSoFar < bytesToRead) {
        const qint64 bytesToReadFromThisBlock = qMin(bytesToRead - readSoFar,
                                                     nextDataBlockSize());
        if (data)
            memcpy(data + readSoFar, readPointer(), bytesToReadFromThisBlock);
        readSoFar += bytesToReadFromThisBlock;
        free(bytesToReadFromThisBlock);
    }
    return readSoFar;
}

/*!
    \internal

    Read an unspecified amount (will read the first buffer)
*/
QByteArray QRingBuffer::read()
{
    if (bufferSize == 0)
        return QByteArray();

    bufferSize -= buffers.constFirst().size();
    return buffers.takeFirst().toByteArray();
}

/*!
    \internal

    Peek the bytes from a specified position
*/
qint64 QRingBuffer::peek(char *data, qint64 maxLength, qint64 pos) const
{
    Q_ASSERT(maxLength >= 0 && pos >= 0);

    qint64 readSoFar = 0;
    for (const QRingChunk &chunk : buffers) {
        if (readSoFar == maxLength)
            break;

        qint64 blockLength = chunk.size();
        if (pos < blockLength) {
            blockLength = qMin(blockLength - pos, maxLength - readSoFar);
            memcpy(data + readSoFar, chunk.data() + pos, blockLength);
            readSoFar += blockLength;
            pos = 0;
        } else {
            pos -= blockLength;
        }
    }

    return readSoFar;
}

/*!
    \internal

    Append bytes from data to the end
*/
void QRingBuffer::append(const char *data, qint64 size)
{
    Q_ASSERT(size >= 0);

    if (size == 0)
        return;

    char *writePointer = reserve(size);
    if (size == 1)
        *writePointer = *data;
    else
        ::memcpy(writePointer, data, size);
}

/*!
    \internal

    Append a new buffer to the end
*/
void QRingBuffer::append(const QByteArray &qba)
{
    if (bufferSize != 0 || buffers.isEmpty())
        buffers.append(QRingChunk(qba));
    else
        buffers.last().assign(qba);
    bufferSize += qba.size();
}

qint64 QRingBuffer::readLine(char *data, qint64 maxLength)
{
    Q_ASSERT(data != nullptr && maxLength > 1);

    --maxLength;
    qint64 i = indexOf('\n', maxLength);
    i = read(data, i >= 0 ? (i + 1) : maxLength);

    // Terminate it.
    data[i] = '\0';
    return i;
}
